/* 
 * File:   GaussianMRFEnergyDoubleton.h
 * Author: hadrien
 *
 * Created on 15 avril 2012, 23:55
 */

#ifndef GAUSSIANMRFENERGYDOUBLETON_H
#define	GAUSSIANMRFENERGYDOUBLETON_H
#include "GaussianProcess.h"
#include <math.h>
#include <cv.h>

#ifndef PI
#define PI 3.141592653589793
#endif

template<typename IMAGEPARAMETERS>
class GaussianMRFEnergyDoubleton {
public:

    typedef IMAGEPARAMETERS Params;
    typedef typename Params::features_vector_t features_vector_t;
    typedef typename Params::label_t label_t;
    typedef GaussianProcess<Params> model_t;

    GaussianMRFEnergyDoubleton(const model_t * model, features_vector_t const * const * observations) {
        _inv_variances = new features_vector_t[Params::getL()];
        _det_variances = new double[Params::getL()];
        _observations = observations;
        _model = model;
        for (int i = 0; i < Params::getL(); ++i) {
            _det_variances[i] = cv::invert(model->getVariance(i), _inv_variances[i], cv::DECOMP_LU);
        }
    }

    ~GaussianMRFEnergyDoubleton() {
        delete[] _inv_variances;
        delete[] _det_variances;
    }

    inline double localEnergy(const int& i, const int& j, const int& label, label_t const * const * labels) const {
        return singleton(i, j, label) + doubleton(i, j, label, labels);
    }

    inline double globalEnergy(label_t const* const* labels) const {
        double singletons = 0.0;
        double doubletons = 0.0;
        int i, j, k;
        for (i = 0; i < Params::getWidth(); ++i)
            for (j = 0; j < Params::getHeight(); ++j) {
                k = labels[i][j];
                // singleton
                singletons += singleton(i, j, k);
                // doubleton
                doubletons += doubleton(i, j, k, labels); // Note: here each doubleton is
                // counted twice ==> divide by
                // 2 at the end!
            }
        return (singletons + doubletons / 2.0)/Params::getHeight()/Params::getWidth();
    }

    inline int getMaximumLikelyhood(const int& i, const int& j) const{
        int mm = 0;
        double e2;
        double e = singleton(i, j, 0);
        for (int r = 1; r < Params::getL(); ++r) {
            if ((e2 = singleton(i, j, r)) < e) {
                e = e2;
                mm = r;
            }
        }
        return mm;
    }
    
    inline double singleton(const int& i, const int& j, const int& label) const {
        double r = ((_observations[i][j] - _model->getMean(label)).dot(_inv_variances[label] * (_observations[i][j] - _model->getMean(label))));
        
        return log(sqrt(pow(2.0 * PI, Params::getN()) * _det_variances[label])) +
                r / 2.0;
    }

private:

    features_vector_t* _inv_variances; //llt()
    double* _det_variances; //determinant()
    const features_vector_t* const* _observations;
    const model_t * _model;

    inline double doubleton(const int& i, const int& j, const int& label, label_t const * const * labels) const {
        double energy = 0.0;

        if (i != Params::getWidth() - 1) // south
        {
            if (label == labels[i + 1][j]) energy -= Params::getBeta();
            else energy += Params::getBeta();
        }
        if (j != Params::getHeight() - 1) // east
        {
            if (label == labels[i][j + 1]) energy -= Params::getBeta();
            else energy += Params::getBeta();
        }
        if (i != 0) // nord
        {
            if (label == labels[i - 1][j]) energy -= Params::getBeta();
            else energy += Params::getBeta();
        }
        if (j != 0) // west
        {
            if (label == labels[i][j - 1]) energy -= Params::getBeta();
            else energy += Params::getBeta();
        }
        return energy;
    }

};

#endif	/* GAUSSIANMRFENERGYDOUBLETON_H */

